/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.worldmodel;

import java.util.LinkedList;
import java.util.Vector;

import robocup.component.DAInamiteConstants;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.actions.Action;
import robocup.component.actions.MoveAction;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.BallInfo;
import robocup.component.infotypes.CoachSayInfo;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.math.RCMath;
import robocup.component.speechacts.MessageFactory;
import robocup.component.speechacts.MessageFactory.MESSAGES;
import robocup.component.worldmodel.particlefilter.BallParticleFilter;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Ball.UPDATE_MODE;
import robocup.component.worldobjects.Player.RELATIVE;

/**
 * BallModel is the part of the WorldModel which implements functions to
 * calculate the state of the ball (position, speed, direction, ...). It is 
 * updated each time an info arrives. 
 * As seen data is given relative to the players position and headdir the 
 * {@link MeModel} update has to be invoked before.
 */
public class BallModel implements IModel {

    // the world for other worldmodel parts methods calls
    private WorldModel world;

    // The ball represented by this BallModel
    private Ball ball;

    // the ball as it was last seen
    private Ball lastSeenBall = Ball.getDummyInstance();

    //the particle filter used for calculating the balls speed and position
    private BallParticleFilter particleFilter = new BallParticleFilter();

    //use the filter
    private boolean ParticlefilterOn = false;

    //a dummy vektor used for less vektor instance creation
    Vektor dummyVektor = new Vektor();

    /**
     * Constructor for BallModel - an object holding the actual ball of the
     * worldmodel and providing the update logic and some useful functions
     * over the ball.
     *
     * @param -
     *            the current world-model (parent)
     */
    public BallModel(WorldModel world) {

        this.world = world;
        this.ball = Ball.getDummyInstance();
    }

    /**
     * updateBallVisual updates the Ball <code>ball</code> from the VisualInfo
     * <code>vInfo</code>. If the ball was not seen, nothing is done as it
     * was already updated by prediction during the BS update.
     *
     * @param info -
     *            the visual info
     */
    public void update(VisualInfo info) {

        BallInfo bi = info.getBall();

        if (bi == null) {
            /**
             * the player doesn't see the ball, we keep its pos/speed calculated
             * in update(SBInfo) method.
             *
             * if the Ball isn't seen but should be, it isn't reliable anymore
             */
            if (this.world.shouldBeSeen(this.ball)) {
                this.ball.setReliable(false);
                this.ball.setSpeedReliable(false);
                this.ball.getSpeed().reset();
            }
            return;
        }

        //the ball pos (relative)
        Vektor ballPosRel = new Vektor(bi.getDistance(), bi.getDirection());

        //the ball pos (absolute)
        Vektor ballPosAbs = this.world.getSelfRef().getPosAbs(ballPosRel,
                RELATIVE.TO_HEAD);

        // if someone is dribbling we already know the ball as it was communicated
        // we believe more in the heared info instead of the seen one as we assume
        // that the dribbling player is nearer to the ball having less noisy
        // information
        if (ballPosRel.getLength() > 3 && this.world.getSelfRef().getLastHearBodyCycle() == this.world.getBodyCycle() && this.world.getSelfRef().getLastHearedMessageType() == MESSAGES.DRIBBLE_PPOS && ballPosAbs.getDistance(this.ball.getPosition()) < ballPosRel.getLength() * 0.1 + 1.2) {
            return;
        }

        // if someone is passing we already know the ball as well
        if (ballPosRel.getLength() > 10 && this.world.getSelfRef().getLastHearBodyCycle() == this.world.getBodyCycle() && this.world.getSelfRef().getLastHearedMessageType() == MESSAGES.KICKED_BALL && ballPosAbs.getDistance(this.ball.getPosition()) < ballPosRel.getLength() * 0.1 + 1.2) {
            return;
        }

        Vektor ballSpeed = new Vektor();
        boolean isSpeedReliable = true;

        // the velocity is written into ballSpeed
        this.world.calculateVelocity(bi, ballSpeed);

        //if the ball's velocity is high we must be in PLAY_MODE=PLAY_ON
        //as we sometimes miss play_mode change infos we set this here
        if (!ballSpeed.isEmpty() && this.world.getPlayMode() != PLAY_MODE.PLAY_ON && ballSpeed.getLength() > 1.0) {
            if (!PLAY_MODE.META_PENALTY_OWN.contains(world.getPlayMode()) && !PLAY_MODE.META_PENALTY_OTHER.contains(world.getPlayMode())) {
                this.world.correctPlayMode(PLAY_MODE.PLAY_ON);
            }
        }

        // if the ball is far or behind us there might not be seen it's velocity
        // the velocity is there approximated as the way between new and last seen
        // position divide by the number of cycles the ball was not seen corrected
        // by the fact that the ball speed decays
        if (ballSpeed.isEmpty() && (this.world.getPlayMode() == PLAY_MODE.PLAY_ON || (this.world.getPlayMode() == PLAY_MODE.GOAL_KICK_OWN && this.world.getNumber() == 1)) && !this.world.collided()) {

            isSpeedReliable = false;
            ballSpeed.copy(ballPosAbs);
            ballSpeed.subFromThis(this.lastSeenBall.getPosition());

            double timeDiff = this.world.getBodyCycle() - this.lastSeenBall.getBodyCycle();

            if (timeDiff == 1) {
                ballSpeed.subFromThis(this.world.getSelfPositionEstimationError());
            }

            double way = ballSpeed.getLength();
            double arrivalSpeed = (way / RCMath.calculateRow(SConf.getInstance().BALL_DECAY, timeDiff)) * Math.pow(SConf.getInstance().BALL_DECAY, timeDiff);

            arrivalSpeed = Math.min(SConf.getInstance().BALL_SPEED_MAX,
                    arrivalSpeed);

            ballSpeed.setLength(arrivalSpeed);
        }

        //we collided, or the play_mode isn't PLAY_ON -> the speed was already set
        // after the BSInfo
        if (ballSpeed.isEmpty()) {
            ballSpeed.copy(this.ball.getSpeed());
            isSpeedReliable = false;
        }

        //add ball to particle filter
        if (this.ParticlefilterOn && !(!isSpeedReliable && ballPosRel.getLength() < 3) && this.world.getPlayMode() == PLAY_MODE.PLAY_ON) {
            this.particleFilter.addVI(bi, this.world.getHeadAngle(), this.world.getMyPosition(), this.world.getMySpeed());
            ballPosAbs = this.particleFilter.getPosition();
            if (this.particleFilter.isSpeedInitialized()) {
                ballSpeed = this.particleFilter.getSpeed();
            }
        }

        // if the ball was seen slightly outside due to noise we set it into the
        // field preventing some of the other methods doing strange things
        this.world.setIntoField(ballPosAbs);
        if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON) {
            ballSpeed.reset();
            isSpeedReliable = true;
        }

        // update the ball
        this.ball.setPosition(ballPosAbs);
        this.ball.setSpeed(ballSpeed);
        this.ball.setCycle(this.world.getCycle());
        this.ball.setBodyCycle(this.world.getBodyCycle());
        this.ball.setReliable(true);
        this.ball.setSpeedReliable(isSpeedReliable);

        // set new age, full seen balls include half seen ones
        if (isSpeedReliable) {
            this.ball.setUpdateMode(UPDATE_MODE.SEEN_FULL, world.getBodyCycle());
            this.ball.setUpdateMode(UPDATE_MODE.SEEN_WITHOUT_SPEED, world.getBodyCycle());
        } else {
            this.ball.setUpdateMode(UPDATE_MODE.SEEN_WITHOUT_SPEED, world.getBodyCycle());
        }

    }

    /**
     * updateBallSense updates the Ball <code>ball</code> from the
     * SenseBodyInfo <code>sbInfo</code>
     *
     * @param info -
     *            Info for the update
     */
    public void update(SenseBodyInfo info) {

        //ball was seen in the last cycle -> save it
        if (this.world.getAge(this.ball) == 1) {
            this.lastSeenBall.copy(this.ball);
        }

        //no play_on -> no ball update as we prefer waiting for a visual info
        if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON) {
            return;
        }

        if (this.ParticlefilterOn) {
            this.particleFilter.translatePosition();
        }

        Vector<Action> sentActions = info.getLastSentActions();

        if (sentActions == null) {
            return;
        }

        boolean kicked = false;

        //predict the ball based on the last world and the actions done
        //only kick, tackle, catch and move(goalie) actions have an effect
        for (Action action : sentActions) {

            switch (action.getType()) {

                case KICK:
                case CATCH_BALL:
                case TACKLE:
                    Player lastMe = this.world.getLastMe();
                    this.ball.predictThisAfterAction(lastMe, action);
                    if (this.ParticlefilterOn) {
                        this.particleFilter.resetPosition(this.ball.getPosition(),
                                this.ball.getSpeed());
                    }
                    kicked = true;
                    break;

                case MOVE:
                    if (world.getSelfRef().isGoalie() && this.ball.isCatched()) {
                        // if the goalie moves with the ball -> move the ball
                        MoveAction ma = (MoveAction) action;
                        this.ball.setPosition(new Vektor(Vektor.XY, ma.getX(), ma.getY() + world.getSelfRef().getPConf().PLAYER_SIZE + SConf.getInstance().BALL_SIZE));
                        this.ball.getSpeed().reset();
                        break;

                    }
                default:
                    ;
            }
        }

        //update ball if not kicked using the filter or simple extrapolation
        if (this.ParticlefilterOn && !kicked && this.particleFilter.isSpeedInitialized()) {
            this.ball.setPosition(this.particleFilter.getPosition());
            this.ball.setSpeed(this.particleFilter.getSpeed());
        } else if (!kicked && !world.ballCanBeKickedByOther()) {
            this.ball.extrapolate(1, SConf.getInstance().BALL_DECAY);
        }

        // if visualAge reached DAInamiteConstants.WM_V_AGE_CONFIDENCE, the ball
        // isn't reliable anymore.
        if (world.getAge(this.ball) > DAInamiteConstants.WM_V_AGE_CONFIDENCE) {
            this.ball.setReliable(false);
            this.ball.getSpeed().reset();
            this.ball.setSpeedReliable(false);
        }
        this.ball.setUpdateMode(UPDATE_MODE.EXPECTED, world.getBodyCycle());
    }

    /**
     * This method handles Say-Messages from other players.
     *
     * There are many message types containing ball information. This information
     * is important as it is probably of higher quality as only players near to
     * the ball and/or handling the ball will communicate it.
     *
     * @param psi -
     *            the {@link PlayerSayInfo}
     */
    public void update(PlayerSayInfo psi) {

        //our ball data is better than the communicated info -> don't update
        if ((this.ball.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.META_BASED_ON_INFO) == 0 && this.world.getDistance(this.ball) < 20) || (this.world.getDistance(this.ball) < 3 && this.ball.isReliable())) {
            return;
        }

        MessageFactory mf = world.getMessageFactory();
        LinkedList<MESSAGES> types = mf.getMessageTypes();

        for (int i = 0; i < types.size(); i++) {
            MESSAGES m = types.get(i);

            switch (m) {
                // the ball was kicked by the sender -> always update
                case KICKED_BALL:
                    mf.decodeMessageNumber(i);
                    this.ball.getPosition().copy(mf.getPosition());
                    this.ball.getSpeed().copy(mf.getSpeed());
                    this.ball.setReliable(true);
                    this.ball.setSpeedReliable(true);
                    this.ball.setBodyCycle(this.world.getBodyCycle());
                    this.ball.setCycle(this.world.getCycle());
                    if (this.ParticlefilterOn) {
                        this.particleFilter.resetPosition(this.ball.getPosition(),
                                this.ball.getSpeed());
                    }
                    this.ball.setUpdateMode(UPDATE_MODE.HEARED, world.getBodyCycle());
                    break;

                // the ball was seen by the sender -> update when info is newer
                case COMPLETE_BALL:
                case INTERCEPT_BALL:

                    if (this.world.getAge(this.ball) == 1) {
                        Player speaker = this.world.getExpectedPlayers()[psi.getNumber() - 1];
                        if (speaker.getDistance(this.ball) > this.world.getSelfRef().getDistance(this.ball) - 4) {
                            continue;
                        }
                    }

                    mf.decodeMessageNumber(i);
                    this.ball.getPosition().copy(mf.getPosition());
                    this.ball.getSpeed().copy(mf.getSpeed());
                    if (mf.getAge() == 0 && this.world.getAge(this.ball) > 0) {
                        this.ball.extrapolate(1, SConf.getInstance().BALL_DECAY);
                    }
                    this.ball.setReliable(true);
                    this.ball.setSpeedReliable(true);
                    this.ball.setBodyCycle(this.world.getBodyCycle());
                    this.ball.setCycle(this.world.getCycle());
                    if (this.ParticlefilterOn) {
                        this.particleFilter.resetPosition(this.ball.getPosition(),
                                this.ball.getSpeed());
                    }
                    this.ball.setUpdateMode(UPDATE_MODE.HEARED, world.getBodyCycle());
                    break;

                //the ball was communicated by the fact that a dribbling player said
                //his position, as the player has the ball under control we set the
                //ball's velocity to "0" and it's position in front of the player
                case DRIBBLE_PPOS:
                    mf.decodeMessageNumber(i);
                    /*if (this.world.getSelfRef().getNumber() == 9) {
                        System.out.println("Obdrzal som dribble pos od hraca cislo: " + mf.getNumbers()[0]);
                        System.out.println("Pozicia lopty pred: "+ this.ball.getPosition());
                    }*/

                    //ball is in front of the player
                    this.ball.getPosition().copy(mf.getPositions()[0]);

                    //set ball in front of player in direction of the others goal
                    if (mf.getAge() == 0) {
                        this.ball.getPosition().addToThis(new Vektor(-0.5, 0));
                    } else {
                        this.ball.getPosition().addToThis(new Vektor(0.5, 0));
                    }

                    // -> set speed to 0
                    this.ball.getSpeed().reset();
                    this.ball.setReliable(true);
                    this.ball.setSpeedReliable(false);
                    // if we dribble the next ball is sent (age code =1)
                    // if an opponent dribbles the seen ball is sent (code = 0)
                    this.ball.setBodyCycle(this.world.getBodyCycle() - 1 + mf.getAge());
                    this.ball.setCycle(this.world.getCycle() - 1 + mf.getAge());
                    if (this.ParticlefilterOn) {
                        this.particleFilter.resetPosition(this.ball.getPosition(),
                                this.ball.getSpeed());
                    }
                    this.ball.setUpdateMode(UPDATE_MODE.HEARED, world.getBodyCycle());

                    /*if (this.world.getSelfRef().getNumber() == 9) {
                        System.out.println("Pozicia lopty po: "+ this.ball.getPosition());
                    }*/
                    break;

                default:
                    ;
            }

        }
    }

    /**
     * This method handles Say-Messages from the coach.
     *
     * @param csi -
     *            the Say-Information!
     */
    public void update(CoachSayInfo csi) {

        MessageFactory mf = world.getMessageFactory();
        LinkedList<MESSAGES> types = mf.getMessageTypes();

        for (int i = 0; i < types.size(); i++) {
            MESSAGES m = types.get(i);

            switch (m) {

                case TRAINER_BALL_POS:
                    mf.decodeMessageNumber(i);
                    this.updateBallPosFromCSI(mf.getAge(), mf.getPosition());
                    break;

                case TRAINER_BALL_SPD:
                    mf.decodeMessageNumber(i);
                    this.updateBallSpdFromCSI(mf.getAge(), mf.getSpeed());
                    break;

                case BALL_POS:
                    mf.decodeMessageNumber(i);
                    this.updateBallPosFromCSI(mf.getAge(), mf.getPosition());
                    break;
            }
        }
    }

    /**
     * updates the ball after a play mode info. Some play_modes
     * (i.e. BEFORE_KICK_OFF) tell us where the ball is.
     *
     * @param info -
     *            Info for the update
     */
    public void update(PlayModeInfo aInfo) {

        switch (((PlayModeInfo) aInfo).getPlayModeType()) {
            case PLAY_ON:
                this.ball.setCatched(false);
                break;
            case PENALTY_SETUP_OWN:
            case PENALTY_SETUP_OTHER:
            case CORNER_KICK_OWN:
            case CORNER_KICK_OTHER:
            case GOAL_KICK_OWN:
            case GOAL_KICK_OTHER:
                this.ball.getSpeed().reset();
                break;

            case KICK_OFF_OWN:
            case KICK_OFF_OTHER:
                if (this.ball.getPosition().getDistance(new Vektor()) > 0.3) {
                    this.ball.getPosition().reset();
                }
                this.ball.getSpeed().reset();
                break;

        }
    }

    /**
     * updates the ball after a referee info.
     *
     * @param info -
     *            Info for the update
     */
    public void update(RefereeInfo info) {

        switch (((RefereeInfo) info).getRefereeMessageType()) {
            case FOUL_OWN:
            case FOUL_OTHER:
            case PENALTY_FOUL_OWN:
            case PENALTY_FOUL_OTHER:
            case BACKPASS_OTHER:
            case BACKPASS_OWN:
            case CATCH_FAULT_OTHER:
            case CATCH_FAULT_OWN:
            case DROP_BALL:
            case FREE_KICK_FAULT_OTHER:
            case FREE_KICK_FAULT_OWN:
            case OFFSIDE_OTHER:
            case OFFSIDE_OWN:
            case HALF_TIME:
                this.ball.getSpeed().reset();
                this.ball.setCatched(false);
                break;

            case GOALIE_CATCH_BALL_OWN:
                this.ball.getSpeed().reset();
                this.ball.setCatched(true);
                break;
            case GOALIE_CATCH_BALL_OTHER:
                this.ball.setCatched(true);
                this.ball.setReliable(false);
                break;
        }
    }

    /**
     * <code>getSpeed</code>
     *
     * @return The ball's velocity. Note that if you perform operations on this
     *         Vektor that change it, you need to clone it first!
     */
    public Vektor getSpeed() {

        return this.ball.getSpeed();
    }

    /*
     * (non-Javadoc)
     *
     * @see robocup.component.worldobjects.DynamicObject#isReliable()
     */
    public boolean isReliable() {

        return this.ball.isReliable();
    }

    /*
     * (non-Javadoc)
     *
     * @see robocup.component.worldobjects.StaticObject#getCycle()
     */
    public int getCycle() {

        return this.ball.getCycle();
    }

    /**
     * <code>getPosition</code>
     *
     * @return Returns the ball's position. Note that if you perform operations
     *         on this Vektor that change it, you need to clone it first!
     */
    public Vektor getPosition() {

        return this.ball.getPosition();
    }

    /**
     * @return Returns the ball. (REF!!)
     */
    public Ball getBallRef() {

        return this.ball;
    }

    /**
     * the last seen ball.
     *
     * @return
     */
    public Ball getLastSeenBall() {

        return this.lastSeenBall;
    }

    /**
     * This method acualizes the balls position after a players message, if he
     * has seen it more recently,
     *
     * @param age
     * @param position
     */
    private void updateBallPosFromCSI(int age, Vektor position) {

        this.ball.setBodyCycle(this.world.getBodyCycle());
        this.ball.setCycle(this.world.getCycle());
        this.ball.setReliable(true);
        this.ball.getPosition().copy(position);
    }

    /**
     * This method acualizes the balls speed after a players message, if he has
     * seen it more recently,
     *
     * @param age
     * @param position
     */
    private void updateBallSpdFromCSI(int age, Vektor speed) {

        this.ball.setBodyCycle(this.world.getBodyCycle());
        this.ball.setCycle(this.world.getCycle());
        this.ball.setReliable(true);
        this.ball.getSpeed().copy(speed);
        this.ball.setSpeedReliable(true);
    }

    /**
     * resets the particle filter in casewe collided with the ball.
     *
     */
    public void resetFilterAfterCollisionWithBall() {
        this.particleFilter.resetPosition(this.ball.getPosition(), this.ball.getSpeed());
    }
}
